﻿using System;
using System.Diagnostics;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Nature.Numerics.Basic
{
    public class SoneNewton<TFunction> where TFunction : ISoneFunction
    {
        #region Nested Types
        class NewtonAnchor
        {
            //public SoneNewton Solver;
            //            public DateTime DateTime;
        }
        #endregion

        #region Private Fields
        NxN matrix;
        ISoneApproximation m_linearizible;
        Vector dx, y, ytmp;
        Vector absTol, relTol;
        Vector lowerLimit, upperLimit;
        Vector convergionsRadius;
        Vector absoluteConvergionsRadius;
        Vector relativeConvergionsRadius;

        [DebuggerBrowsable(DebuggerBrowsableState.Never)]
        int m_maxNumberOfLinearApproximationReuse;

        [DebuggerBrowsable(DebuggerBrowsableState.Never)]
        int m_linearApprocimationReuse;

        [DebuggerBrowsable(DebuggerBrowsableState.Never)]
        int m_numberOfIterations;
        #endregion

        public SoneNewton(TFunction function)
        {
            Function = function;
            Nva = function.Nva;
            matrix = new NxN(Nva);
            m_linearizible = Function as ISoneApproximation;
            m_maxNumberOfLinearApproximationReuse = Nva;
            dx = new Vector(Nva);
            y = new Vector(Nva);
            ytmp = new Vector(Nva);
            absTol = new Vector(Nva, 1.0e-5);
            relTol = new Vector(Nva, 1.0e-5);
            lowerLimit = new Vector(Nva, -double.MaxValue);
            upperLimit = new Vector(Nva, +double.MaxValue);
            absoluteConvergionsRadius = new Vector(Nva, double.MaxValue);
            relativeConvergionsRadius = new Vector(Nva, double.MaxValue);
            convergionsRadius = new Vector(Nva);
            m_linearApprocimationReuse = int.MaxValue;
        }

        #region Iterator Configurations

        public void SetAbsoluteTolerance(double value)
        {
            if (value < double.MinValue)
            {
                throw new System.ArgumentOutOfRangeException();
            }
            for (int iva = 0; iva < Nva; ++iva)
            {
                absTol[iva] = value;
            }
        }

        public void SetAbsoluteTolerance(double[] values)
        {
            for (int iva = 0; iva < Nva; ++iva)
            {
                if (values[iva] < double.MinValue)
                {
                    throw new System.ArgumentOutOfRangeException();
                }
                absTol[iva] = values[iva];
            }
        }



        public void SetRelativeTolerance(double value)
        {
            if (value < 1.0e-14)
            {
                throw new System.ArgumentOutOfRangeException();
            }
            for (int iva = 0; iva < Nva; ++iva)
            {
                relTol[iva] = value;
            }
        }

        public void SetRelativeTolerance(double[] values)
        {
            for (int iva = 0; iva < Nva; ++iva)
            {
                if (values[iva] < double.MinValue)
                {
                    throw new System.ArgumentOutOfRangeException();
                }
                relTol[iva] = values[iva];
            }
        }





        public void SetLowerLimit(double value)
        {
            if (value < (-double.MaxValue))
            {
                throw new System.ArgumentOutOfRangeException();
            }
            lowerLimit = lowerLimit.IsNull ? new Vector(Nva) : lowerLimit;
            lowerLimit.Assign(value);

        }

        public void SetLowerLimit(double[] values)
        {
            lowerLimit = new Vector(values, true);
        }



        public void SetUpperLimit(double value)
        {
            upperLimit = new Vector(Nva, value);
        }

        public void SetUpperLimit(double[] values)
        {
            upperLimit = new Vector(values, true);
        }

        public void SetAbsoluteConvergenceRadius(double value)
        {
            absoluteConvergionsRadius.Assign(value);
        }
        public void SetRelativeConvergenceRadius(double value)
        {
            relativeConvergionsRadius.Assign(value);
        }


        public void SetAbsoluteConvergenceRadius(double[] values)
        {
            absoluteConvergionsRadius = new Vector(values);
        }

        public void SetRelativeConvergenceRadius(double[] values)
        {
            relativeConvergionsRadius = new Vector(values);
        }

        #endregion

        /// <summary>
        /// Number of variables
        /// </summary>
        public int Nva { get; private set; }

        public TFunction Function { get; private set; }

        public int MaxNumberOfLinearApproximationReuse
        {
            get { return m_maxNumberOfLinearApproximationReuse; }
            set
            {
                m_maxNumberOfLinearApproximationReuse = Math.Max(value, 1);
            }
        }

        public int NumberOfIterations { get; private set; }

        public void Solve(double[] x, ref object anchor)
        {
            // scaled component-averaged Newton increments
            double previousAveragedIncrement = double.MaxValue;
            double currentAveragedIncrement = double.MaxValue;
            double damping, tmp;

            NumberOfIterations = 0;
            // number of variables
            bool solved = false;
            while (false == solved)
            {
                if (m_linearApprocimationReuse >= MaxNumberOfLinearApproximationReuse)
                {
                    Linearize(x);
                    m_linearApprocimationReuse = -1;
                }
                // sub-iterations using the same linear approximation
                bool converging = true;
                for (; m_linearApprocimationReuse < MaxNumberOfLinearApproximationReuse; ++m_linearApprocimationReuse)
                {
                    if (false == converging) { break; }

                    m_numberOfIterations += 1;

                    #region Determining the convergions radius
                    for (int i = 0; i < Nva; ++i)
                    {
                        convergionsRadius[i] =
                            absoluteConvergionsRadius[i] +
                            Math.Abs(x[i] * relativeConvergionsRadius[i]);
                    }
                    #endregion

                    #region Damping Procedure
                    Function.Compute(x, dx);
                    matrix.Solve(dx);
                    m_linearApprocimationReuse++;
                    damping = 1.0;
                    for (int i = 0; i < Nva; ++i)
                    {
                        tmp = Math.Abs(dx[i]);
                        if (tmp > convergionsRadius[i])
                        {
                            tmp = convergionsRadius[i] / tmp;
                            if (damping > tmp) damping = tmp;
                        }
                    }
                    if (1.0 > damping) dx.Multiply(damping);
                    solved = (1.0 == damping);
                    #endregion

                    #region Checking if the problem is solved
                    for (int i = 0; i < Nva && true == solved; ++i)
                    {
                        tmp = absTol[i] + Math.Abs(relTol[i] * x[i]);
                        solved = (Math.Abs(dx[i]) < tmp);
                    }
                    if (true == solved)
                    {
                        x.Add(dx);
                        NumberOfIterations++;
                        break;
                    }
                    #endregion

                    x.Add(dx);
                    NumberOfIterations++;
                    //for (int iva = 0; iva < Nva; ++iva)
                    //{
                    //    x[iva] = Math.Min(x[iva], upperLimit[iva]);
                    //    x[iva] = Math.Max(x[iva], lowerLimit[iva]);
                    //}

                    #region Checking if iterations are converging
                    currentAveragedIncrement = 0.0;
                    for (int i = 0; i < Nva; ++i)
                    {
                        tmp = dx[i] / convergionsRadius[i];
                        currentAveragedIncrement += tmp * tmp;
                    }
                    currentAveragedIncrement = Math.Sqrt(currentAveragedIncrement);
                    converging = (currentAveragedIncrement < previousAveragedIncrement);
                    previousAveragedIncrement = currentAveragedIncrement;
                    #endregion
                }
            }//while (false == solved)
        }

        private void Linearize(double[] x)
        {
            //1) linearize the systems
            if (m_linearizible != null)
            {
                matrix.Linearize(m_linearizible, x, y, absTol, relTol, lowerLimit, upperLimit, ytmp, -1);
            }
            else
            {
                matrix.Linearize(Function, x, y, absTol, relTol, lowerLimit, upperLimit, ytmp, -1);
            }
            matrix.Decompose();
        }


    }

    public class SoneNewton : SoneNewton<ISoneFunction>
    {
        public SoneNewton(ISoneFunction function) : base(function) { }
    }
}
